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ABSTRACT 


Two different algorithms to navigate an AUV within a charted environment are 
presented. They use sonar returns and a local map together with the dynamic model to 
estimate the vehicle’s position and acceleration at all times. Kalman filtering techniques 
are used to compute the estimates. The main difficulty is the presence of uncharted 
obstacles, which are identified by the potential function algorithm. Results from the 
application of the potential function algorithm in a pool using Tritech ST725 high 
resolution sonar show the feasibility and robustness of the potential function approach to 


the navigation problem. 


Accesion For 


NTIS CRA&I 
DTIC TAB 
Unannounced 
Justification 


Distribution | 


Availability Codes 
Avail and/or 
Dist Special 


A-| 





iti 





TABLE OF CONTENTS 


I. INTRODUCTION ........0. 000 cece cece eeceeeeeeeeeeees i 
Rs 3GEHNERAD 5-82 645 cats Ga die ice our ede mead 1 

B. AIMOFTHESTUDY .................c0 cc eeeeeeee 2 

C. METHOD OF APPROACH ..............--- eee e ee eee 2 

Il. A GEOMETRICAL MODEL OF ENVIRONMENT ............... 5 
A SGENERAL Hf 38 pt Sona diduin ste Rhea kbs woe 5 

B. DISCRETE KALMAN FILTER EQUATIONS .............. 5 

C. LINBARIZATION ............ 0000 ceee cece eee eeee 8 

D. EXTENDED KALMAN FILTER ...................05- 9 

~B  MODELLING FOR PLANAR MOTION OF THE AUV ....... il 

F. SIMULATION RESULTS ..................-0- 000005 15 

I. POTENTIAL FUNCTION APPROACH FOR ENVIRONMENT ...... 21 
AL GENERAL o.oo eee cece cece ccc eev cues 21 
B. MODELLING ASSUMPTIONS ...............-...0.. 21 

C. SIMULATION RESULTS ........... 000 ceceeeeeeeee 23 

D. ESTIMATION OF SYSTEM PARAMETERS .............. 32 





1. Model Structures ............0...0...-2. 2.0002 eee 32 


2. ARMAX Models and Difference Equations ............. 33 

3. Recursive Estimation ..............0-...0 000 eee 34 

4. Simulation Results ............................ 36 

IV. SUMMARY, CONCLUSIONS, AND RECOMMENDATIONS ....... 47 
A. SUMMARY ............. 0.02. .ceeeeeee e ee eeene 47 

B... - CONCLUSIONS icc). 6.8 dow ee He BOS BR 8 a Hots 47 

C. RECOMMENDATIONS ..................2002 2 cease 48 
APPENDIX Be seas oe ana eS Ok GE OANA B A he BORE 49 
A. PROGRAM STRUCTURE ...................000005 49 


B. PROGRAM LISTING FOR SIMULATIONS IN CHAPTER TIT ... 50 


A. PROGRAM STRUCTURE .................200000.. 56 


B. PROGRAM LISTING FOR SIMULATIONS IN CHAPTER IIT... 57 


LIST OF REFERENCES > (2646.6 be eta buiws Nw ae Saws eeu 65 


INITIAL DISTRIBUTION LIST ................ 0.0.2.2 .0 008. 67 


I would like to express my thanks to Professor Roberto Cristi for his help and 
encouragement during this study. His technical expertise and insight were invaluable to 
me. His patience, maturity, and understanding of human nature helped me to overcome 


many difficulties. 


I would like to offer special thanks to my wife Dilek, and my son Onuralp for 


their patience, support, encouragement and sacrification. I dedicate this thesis to you. 








I. INTRODUCTION 


A. GENERAL 

Stated most simply, the problem of navigation can be summarized by the following 
three questions: "where am I?", "where am I going?", and “how should I get there?". 
The first question is one of localization: “how can I work out where I am in a given 
environment, based on what I can see and what I have previously been told?". The 
second and the third questions are essentially those of specifying a goal and being able 
to plan a path that results in achieving this goal. Investigation of the latter two questions 
usually comes under the domain of path planning and obstacle avoidance [Ref. 1}. In this 
thesis, we are principally concerned with the first question, localization, since a robust 
and reliable solution to this problem is essential to answering the remaining two 
questions. 

An Autonomous Underwater Vehicle is a type of Unmanned Underwater Vehicle 
(UUV), that is not limited by the need for local human control. The freedom from 
requiring an external control interface theoretically allows this type of vehicle to perform 
a great range of missions [Ref. 2]. To widen the range of application of an AUV, it is 
necessary to develop systems with high levels of autonomy and to operate in unstructured 
environments with little a priori information. To achieve this degree of independence, 


the AUV must have an understanding of its surroundings, by acquiring and manipulating 


a rich model of its environment of operation [Ref. 3]. While autonomy has clear 


advantages, it requires a sophisticated level of on board processing ability. 


B. AIM OF THE STUDY 

This thesis is concerned with the navigation problem of an AUV in the horizontal 
plane. For any navigation system to be useful, the accuracy of the instruments has to be 
such that the navigator’s error is within the required tolerance of the vehicle which relies 
on the navigation system. However, accurate sensors such as high quality inertial 
navigation systems and doppler sonars tend to be large and expensive, and unsuitable to 
a small vehicle. It is the aim of this thesis to study the feasibility of combining 
measurements from the sonar device on board to generate relatively good position 


estimates over a short time interval. 


C. METHOD OF APPROACH 

This thesis is concerned with the short-range navigation problem for the NPS 
testbed AUV. Being limited in complexity and cost, the navigator has to rely on the 
available hardware. Also, because the system needs only to operate over short ranges, 
the effects of the earth’s orbit and rotation can be neglected. This approximation also has 
the effect of simplifying the model to be built. 

The approach taken in this thesis to solve the navigation problem in the horizontal 
plane is to use a nonlinear dynamic model of the vehicle to filter the sonar range returns 
for estimation of the vehicle’s position. The measurements obtained from sonar and 


nonlinear model are run through the Kalman filter to calculate the Kalman gain. Then, 








the Kalman gain is used to make the next position estimation of nonlinear model. This 


approach is diagramed in Figure 1.1. The filter to be used is an Extended Kalman filter. 





Figure 1.1 Kalman Filtering Concept 


The Kalman filter theory, which appeared in the early 1960’s, was recognized as 
an ideal solution to the navigation data processing problems for navigation. These data 
processing problems can be adapted nicely into the necessary Kalman filter assumptions, 
which means that the estimates of the desired navigation outputs are, in fact, very nearly 
optimum. This gives the engineer confidence that the system is the best that can 
reasonably be expected. Also, the recursive form of the filter is convenient. A new 
optimum estimate can be made very shortly after each new measurement is obtained. Nor 
is it necessary to store a great amount of data or invert a large matrix, as might be 
necessary in more conventional, least-squares fitting techniques [Ref. 4]. 

Navigation systems are among the most popular areas for the application of Kalman 


filtering. Most of the major navigation system manufacturers have developed or proposed 


systems based on Kalman fiitering techniques, and they are being used in several vehicles 
in operational use. i.alman filtering has now become an integral part of almost any 
navigation system. The reasons for the popularity of Kalman filtering in navigation 
Syst-ms are not hard to find. There are at least three major complementary factors that 
have come together at the proper time. These factors are an increased need for self 
contained navigation systems, the mathematical tools to design, and the necessary 
equipment to implement [Ref. 4]. 

This thesis is presented in four parts. Chapter II discusses background and the 
theory behind the navigator design and presents some results for a model designed to 
operate in a pool without any obstacles in the operation environment. Chapter II 
discusses the details of potential function technique used in simulations and the 
autoregressive extended (ARX) model based parameter estimation. Finally, Chapter IV 
summarizes the results of this research and contains conclusions and recommendations 


for application and further study. 


i. A GEOMETRICAL MODEL OF ENVIRONMENT 


A. GENERAL 

In this chapter, we set up the nonlinear model for the dynamics of an AUV and the 
model for sonar measurements.Then, we define the experiment in a poo! without 
obstacles and implement Kalman filtering techniques for estimating the position of the 


vehicle. In the estimation process, we use the sonar range measurements only. 


B. DISCRETE KALMAN FILTER EQUATIONS 
The Kalman filter estimates the state of a system given a set of known inputs to the 
system and a set of measurements [Ref. 5]. The system is assumed to be driven by both 


a known input and an unknown random input: 
x(k+1)=Ox(k) +4 ,u(k)+A,w(k) , (2.1) 


where u(k) is the known input, w(k) is the random input called the plant driving noise, 
x(k) is the state vector, ® is state transition matrix, A, is known input matrix, and A, is 
random input matrix. We assume that w(k) is white noise. 


The measurements of the system are related to the state by 


y(k)=Hx(k)+v(k) , (2.2) 
where y(k) is the measurement, H is the measurement matrix, and v(k) is the random 


measurement noise. We assume that v(k) is white noise. 


The solution of the estimation problem can be shown to have the following form: 


R(k+1 [k+1)=R(k+1 |) +G(k+ DLy(k+ 1) -p(K+1)] , (2.3) 
where G(k+1) is the Kalman filter gain at the given specific time. 
The Kalman filter gain G(k+1) is found by applying the orthogonality principle 


which leads to the recursive equations: 


P(k+1 [k)=@P(k|k)®7+4,WA,7 
G(k+1)=P(k+1 |VH THP(R+1 |TV)" 
P(k+1|k+1)=[[-G(K+1)HP(k+1|k) , 
where P is the covariance matrix of the estimation error. The following is the summary 
of the Kalman filter equations in the order in which they are implemented throughout this 


thesis: 


P(k+1|k)=@P(k|K)O™+A,WA,7 


G(k+1)=P(k+1|KH "[HP(k+1|QH T+)" 
P(k+1|k+1)=[1-G(k+ 1) AYP(k+1/4) 


The Filter Equations: 


L(K+1 |k)=OR(K|KE)+A uD) 
Sk +1 |k)=HR(k+1|k) 
RK +1 [k+1)=K(K +1 [K)+GK+ I fy(k+ 1) -H(k+1 [1] 





This is known as the predictor-corrector form of the Kalman filter in which the 
next state is predicted using the state space model and the measurement is used to correct 
the prediction. [Ref. 5] 

The W matrix is the covariance of the system’s noise, while the A, matrix 
describes the way this noise enters the system. If W is diagonal, then the disturbances 
are assumed to be independent, otherwise they are correlated, as described by the off- 
diagonal terms in W. [Ref. 5] 

The V matrix describes measurement noise from the sensors. The Kaiman filter 
considers the measurement noise to enter all the individual measurements. As with the 
measurement noise covariance matrix, if V is large, then the measurements are e :pected 
to deviate more from the states being measured, and the Kalman filter will rely more on 
the predicted state than on the measurements.[Ref. 5] 

The initial P matrix affects the dependence the Kalman filter has on the initial 
conditions. Large values in initial P mean that the filter will not depend on the initial 
conditions, but will instead give more weight to the measurements. This allows the 
estimated state to change rapidly as the filter goes through its transient stage. In steady 
state, however P has no effect on the Kalman filter because it approaches a constant 
value that is dependent only on the system and the noise covariance matrices. [Ref. 5] 

Because the gain equations do not depend directly on time or on the state trajectory 
in a LTI system, the gain can be calculated a priori and recalled from memory as 
needed. There is no need for real-time gain computation. Moreover, in the time invariant 


case the gain matrix approaches a steady-state value, which is determined by the system 


equations and the W and V matrices through the associated Kaiman filter equations. In 
many cases, using the steady-state gain matrix instead of the time-varying gain matrix 
gives satisfactory results in a reduced-complexity algorithm which does not take into 


account prior knowledge of the initial conditions. [Ref. 5] 


C. LINEARIZATION 

Thus far, the discussion has been limited to linear systems; however, many systems 
in which the control engineer is interested are nonlinear. Nonlinear system models are 
more general than linear models and can contain a wide variety of nonlinear 
characteristics for which limited analytic tools exist. In general, nonlinear systems do not 
exhibit the properties of homogeneity or superposition, and many include transcendental, 
trigonometric or other nonlinear functions (Ref. 6]. Such is the case with the model 
chosen for this thesis. 

A method for working with nonlinear systems is to linearize them using a truncated 
Taylor series approximation. The Taylor series approximates a function around a given 
point as an infinite sum of weighted, analytically determined, partial derivatives. A 


general Taylor series around the point x, is given by: 





Six)= > mo -x,) (2.7) 


a=1 


A truncated Taylor series only uses a few of the terms of the sum. In order to 
realize a linear function from the Taylor series expansion of a nonlinear one, the series 


must be truncated at the first term. This model is general and can be applied to any 





nonlinear system, and it will be valid in a region surrounding the linearization point [Ref. 


7). 


For the case of a continuous-time dynamic system represented in state-space form, 
which, in general, is not a function of a single variable, but is rather a function of time. 
the input vector, and the past state vector, the Taylor series is defined over the partial 
derivatives of each of the independent variables. In the case of a time-invariant system, 
the series is expanded about an operating point described by the state, x,, and a 


corresponding input, u,. A nonlinear state space equation can then be approximated as 


=X.) + Hoot x) Teote) ee -u,) (2.8) 


where f is the nonlinear system function. This notation implies that the partial derivatives 
are constant terms, calculated analytically and evaluated at x,, and u, (Ref. 8). 

Just as a nonlinear system equation can be expanded using a Taylor series, and 
linearized, a nonlinear measurement equation can also be expanded and linearized. The 


formulation for this expansion is similar to the equation 2.8. 


D. EXTENDED KALMAN FILTER 

The Kalman filter is derived for linear systems with linear measurement equations; 
however, using the linearization techniques described in the last section, this filter can 
find application to general, nonlinear systems. This is known as the Extended Kalman 
filter. The Extended Kalman filter is suboptimal and may suffer convergence and stability 


problems, but it has been shown to be useful in variety of applications. [Ref. 5] 





The form of the Extended Kalman filter equations are essentially similar to those 
of the Linear Kalman filter. The nonlinear model is used to predict the state and the 
nonlinear measurement is used to correct the prediction. The most significant difference 
between the Extended Kalman filter and the Linear Kalman filter is the gain equations. 
Rather than using ® (the state transition matrix), A (the input matrix), and H (the 
measurement matrix) matrices to calculate the gain, the Extended Kalman filter uses the 
linearized model of the nonlinear system. It uses the partial derivatives of the nonlinear 
state equations and the nonlinear measurement equations. These partials are evaluated at 
each estimated state. As such, the gain, G, cannot be computed in advance, because it 
is dependent on both the state trajectory and the input history. 

Calculating partial derivatives of the nonlinear system and measurement equations 
for each measurement as well as calculating the Kalman filter gain matrix is a 
computational burden. In order to overcome this problem, it may be possible to use gain 
matrices calculated in advance and by choosing discrete points in the system’s state space 
about which to linearize the nonlinear system. Practically, in choosing the points, it is 
convenient that only some of the system states are varied while others are kept constant. 
Using the chosen points, several linear approximations to the nonlinear system are 
calculated and then used to calculate the steady-state Kalman filter gain matrix associated 
with those particular points. Once the gains are calculated, the Extended Kalman filter 
is implemented by determining which of the chosen linearization points are closest to the 
current estimated state and the corresponding gain matrix is used in the Extended Kalman 


filter equation as given as the last of Equations 2.6. [Ref. 5] 
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C] In using the Extended Kalman filter, nonlinearities are modeled as plant noise. This 
means that the A, matrix is usually the identity matrix. Furthermore, because the 
nonlinear effects are not included in the gain equations, the Extended Kalman filter can 
be very sensitive to the W and V matrices. Choosing improper values can make an 
Extended Kalman filter unstable. The values chosen for these matrices should not 
necessarily correspond to the actual noise expected in the system or in the measurements, 
but rather they must be made large enough to provide a robust prediction in spite of the 


nonlinear effects. 


E. MODELLING FOR PLANAR MOTION OF THE AUV 
The basic component of the estimation process is a state vector whose value at any 


time t is given by: 


ely (2.9) 


where (x,,y,) defines the position of the vehicle with speed v, heading 6, yaw rate 6 . The 
subscript "v" is used to distinguish the position of vehicle from the state vector x and 


measurement y. 
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Input to the system are propeller speed, u,, and vertical rudder command, uw). 


u 
-| (2.10 ) 


The differential equations forming to our model are as follows: 


v cos6 
v sin 
-a v+B u, (2.11) 
6 


-y 6+0 4, 


x= 


where a, 8, y, and o are constants depending on the dynamics of the vehicle. 

As we can see, equations 2.11 are nonlinear and they have to be linearized for 
further analysis. Now let us apply truncated Taylor series expansion to the first two 
equations. In the following equations the subscript "o" indicates the current value of the 


particular state around which we linearize the model, 


H%,(v,.8,)) AE,(V,:0,)) 


4,-v,0080,* = (y-v,)s 2 -6,) (2.12) 
9,=»,n0, + PHO (yy) Be Oe -6,) (2.13) 


where v, and 6, are the current speed and heading of AUV respectively. Now if we 


follow the algebra involved in equations 2.12 and 2.13 we can find the following results: 
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%,=veos6, -6v,sin6, +6,v,sin6, (2.14) 


y,=vsin6, +6v,cos6, -6,v,cos6, (2.15 ) 


The last two terms in the equations 2.14 and 2.15 are almost identical; they differ in one 


sign. Now the following approximations can be made: 


-~ 


%,=vcos6 (2.16 ) 


v Qo 


y,=vsind,, (2.17) 


In the model introduced above, sonar range is the only measurement that depends 
on the current position and orientation of the AUV with respect to the environment. The 
“; parameter is sonar heading which is measured with respect to the x-axis of the vehicle 


and defines the angle between x-axis and the sonar beam at the measurement time. 


Y=BR wy) (2.18 ) 


In this study, we address the problem of navigating the vehicle in a geometric 
environment, like a pool or tank with known dimensions L, by L,. Figure 2.1 shows the 
setup for range measurements . The » parameters define the particular angle computed 
between x-axis and comers of the pool, taking the current position of the vehicle as the 
origin. Range r, sonar heading y, and vehicle position (x,, y,) are related by the 


geometry of the environment as 








r=- id : N2SY<1, ( 2.19 ) 
cosy 
pee) sa seren, (2m) 
siny 
L,-x 
ee es ayeyeny (225) 
COSY 
pose . n,SY<n, (2.22 ) 
siny 





Figure 2.1 Determination of sonar range in the pool 


If we call H the measurement matrix defined as 
a4 £900) (2.23) 
dx dy 


then we can compute it from the equations 2.19 through 2.22 as 


% 
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H= Ee 000 o 5 mSY<2, 5 MU SY<9, ( 2.24) 
cosy 


x-|0 -1. 00 o > mSy<mimsr<a, (225) 
Sinry 
In the light of the preceding discussion, the state space representation of the 


particular model can be defined as: 


0 0 cos8#, 0 0] I9 0 
0 0 sind, 0 0| (0 0 

45400 -a 0 ORD Ob+w care) 
00 0 01); 9 


00 0 O-c} Od 


y-Hxt+v ( 2.27 ) 


F. SIMULATION RESULTS 

Figures 2.2 through 2.10 show the simulated vehicle trajectory, as determined by 
the nonlinear model. In all the cases, the vehicle was given a constant propeller speed 
of 140 RPM. It starts from an estimated position when it is at rest initially. After the 
convergence of the Kalman filter to the simulated trajectory, a rudder command is given 
for a tum to be executed. In smooth turns, like shown in Figures 2.2 through 2.10, the 


model is able to track the actual simulated trajectory. 
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The lower left hand comer of the pool is accepted as the position reference point 
for all the runs. There are no obstacles present in the pool, which yields to clear sonar 


returns from the borders of the pool. 


Vehicie 


Estimate ..... 





Figure 2.2 A sample run with initial heading 0° 











Figure 2.4 A sample run with initial heading 45° 
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Figure 2.5 A sample run with initial heading 60° 


Venicte —_ 
Estimate 





Figure 2.6 A sample run with initial heading 90° 
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Figure 2.7 A sample run with initial heading 180° 


Vehicle 
cstimate 





Figure 2.8 A sample run with initial heading 315° 
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Venice — 
Estimate 





Figure 2.9 A sample run with a steady turn 


Vehicle — 
Estimate 





Figure 2.10 A sample run with a complete steady tum 
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Il. POTENTIAL FUNCTION APPROACH FOR ENVIRONMENT 


A. GENERAL 
In this chapter, we will approach the problem of navigation with the same model 
used in Chapter II by using a potential function, which is intended to define the 


environment. 


B. MODELLING ASSUMPTIONS 
The basic component of the estimation process is based on the state vector which 


is defined in equation 2.15. This leads to a dynamic model of the form 


Z,=fz,.u,W) (3.1) 
with z, € R° the state of the vehicle, u € R? the vector of external commands (RPM and 
RUDDER ANGLE) and w, disturbances, modelling errors between model and the actual 
physical dynamics. [Ref. 9] 

The measurement vector consists of dynamic observations of sonar range p at an 
angle a with respect to the longitudial axis of the vehicle with v indicating measurement 


noise 


Y=B(Z,.P,0,V) . (3.2) 


We assume the noise sources w and v to be of zero mean, white, and Gaussian, as 


is standard in this class of problems. The function g is defined by the map of the 











environment, and apart from measurement noise, it is zero when the sonar retum 


information (9, a) is consistent with the vehicle position with respect to the map. The 
criterion we use to choose g is critical for this thesis, and it is based on the use of a 
potential function properly defined. For a vehicle moving on a plane and located by 


position (x,y) and heading 0, the function g is defined as 


g(x,y,8,p,a)=V(x+pcos(8 +«),y+ psin(6+«)) (3.3) 


The vector 
[x,.7,] =[x+ pcos(6 + «),y + psin(6 + «)) (3.4) 


denotes the location of the tip of the sonar range vector in the given reference frame. 
With this definition, function V has to satisfy the following necessary conditions. 
© V is continuous and differentiable 
@ V(x,,y,)=0 if the point (x,,y,) is on a reflecting surface defined on the map 
@ V is uniformly bounded over R? 
Considering a pool of rectangular shape with sides L, by L, units, and by taking 
the lower left corner as the origin of our map and two adjacent sides as axis, we can 


write the potential function V as 


Vix.y) =F, @G-L,)yy-L,)) (3.5) 


We choose F, in equation 3.5 in order to provide boundedness for V. In particular, the 


choice of a "squashing" function such as the sigmoid 
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(3.6) 


for some positive \, will make the potential V satisfy the conditions stated above. 
The parameter in the squashing function has to be chosen as a compromise 
between two conflicting necessities: 


@ large enough to provide a sufficient region of attraction to correct for errors in 
estimates 


@ small enough so that objects which are in the field of operation and not on the map 
are outside the region of attraction 


In the application shown below we use a time varying \ decreasing with time in 
order to accommodate larger errors at the beginning of the estimation processes. The 


estimate of the state z is just a standard Extended Kalman filtering operation, which is 


basically defined in Chapter II. 


C. SIMULATION RESULTS 

The navigation algorithm has been tested on a rectangular pool with sides L, by L,. 
Figure 3.1 shows typical contours of potential function, while Figure 3.2 is the three 
dimensional plot of the potential function over the pool. 

The sonar returns are recorded from several scans spanning the whole 360° circle 
at intervals of 0.9°, thus resulting in 400 sonar returns per scan. The source codes used 
during the simulations are listed in Appendix-B. The programs SCAN5S.M, SIMULS.M, 
VP.M, SIG.M, and DSIG.M are main files listed. The numbers used in the names of 


codes indicates the version number of a main change in program structure. 
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Figure 3.1 The contours of the potential function 
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Figure 3.2 Three dimensional potential function 
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The forward velocity estimate, shown in Figure 3.3, approaches a constant value. 
The estimation error seen in the beginning of this run is induced by the random 
disturbances added to the initial state.The yaw rate estimate, shown in Figure 3.4, and 
the yaw estimate, shown in Figure 3.5, exhibits fairly good estimates. The initial errors 
are due to the initial estimates of the system states and added random disturbances. The 
yaw estimate approaches its actual value after 30 seconds of simulation time, which shows 


us the sensivitivity of the system to the heading estimates. 
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Figure 3.3 Measured and estimated forward velocities 
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MEASURED AND ESTIMATED YAW RATE 
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Figure 3.4 Measured and estimated yaw rate 
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Figure 3.5 Measured and estimated yaw angle of the AUV 
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The results of the position estimation are shown in Figures 3.6 through 3.9, where 
we used the data collected for seven successive 360° scans for each case. In Figures 3.6 
and 3.7, we assume the initial position and orientation of the AUV to be known. In these 
figures the sequence of location of the estimated reflective surfaces is shown for each 
scan, superimposed to the contour of the potential function. The estimated trajectory is 
also shown, which coincides with the actual induced motion of the sonar head. The 
estimates of the reflection from charted objects can easily be distinguished from the 
charted regions, since the value of the potential function is close to one for the uncharted 
and zero for the charted. In Figures 3.8 and 3.9 we repeat the same simulation with a 
significant error in the estimate of the initial location of the sonar head. When the 
original location is unknown, we start with a large enough value of the parameter \ and 
make it decrease exponentially. In Figure 3.9, we notice that until the convergence of the 
Kalman filter we might get some estimates that are strictly outside the map defined by 
the potential function. Then these initial estimates eventually converge to the original 


trajectory of the vehicle. 
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Figure 3.6 Estimation with known initial conditions and zero degrees heading as a start 
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Figure 3.7 Estimation with known initial conditions and a steady turn to portside 
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Figure 3.8 Estimation with unknown initial conditions and steady motion 
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Figure 3.9 Estimation with unknown initial conditions and a steady tum 
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D. ESTIMATION OF SYSTEM PARAMETERS 


The estimation of the system dynamic: is the subject of system identification. 
Identification has many aspects and phases, and it is customary to organize identification 
by considering a certain number of steps each time we encounter an identification 
problem. It is often natural to restrict the complexity of modeling to a certain model 
Structure. The class of models thus adopted often belong to some standard category of 
models such as linear systems or ARMAX model associated with certain mathematical 
properties. Another standard approach often used is to make assumptions as to the 


physical nature of the system or other restrictions that define physical parameterization. 


1. Model Structures 
The approaches to modelling of linear systems and linear regression models 


yields to the following equation, in the regression form as 
My =", yy, (3.7) 
Linear regression identification consists in reformulating various estimation and 


prediction problems, in the form of equation 3.7, which for suitable definitions of the 


observations y,, regressors ¢,, and disturbances v, also applies directly to the model 
Az~y,=BRu,+¥, » (3.8) 

where the discrete time series y, and u, provide data. The term time series here means 

a sequence of data ordered in time [Ref. 10]. The corresponding identification problem 


consists in determining the model structure and parametric estimation of the polynomials 


involved. In many cases, such parameters can be identified by using a linear regression 
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approach. The least-squares solution to the linear regression problems exhibits excellent 


properties in cases where the disturbances at different sampling times are uncorrelated. 
The systematic errors in cases with more complicated spectral disturbance characteristics 
constitute a significant problem. Conversely, the presence of correlated disturbances of 
composition other than white noise renders bias reduction necessary. Efforts to solve 
such problems have given rise to several extensions of linear regression models. In 
particular, Recursive Least-Squares (RLS) Method applied to the estimation of 
Autoregressive Moving Average Extended (ARMAX) models is of central importance 


in this thesis. 
2. ARMAX Models and Difference Equations 
The ARMAX models constitute an important class of difference equations on 
the form 
AQ Y(D=BR uN +CR Ye , (3.9) 


where z" is a time delay and A,B,C are polynomials 
A(z")=1+a,z7'+...+4,27' 
B(z“')=b,+b,z7'+...+b,2 7! (3.10) 


C@)=1+e,z7'+...+¢,2 7 


The ARMAX model, in equation 3.9, is completely defined by the polynomials A, B, 


and C. So we can group all unknown coefficients in an array 


D=[0, 5.008 gsDjyo0eyDgsCpyoersCal 5 (3.11) 


which completely defines the model. 
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The special case of ARMAX model that admits a reformulation to the linear 


regression model is the controlled autoregressive extended model (ARX) 


A(z~')y,=B@ ua, +w, , (3.12) 
where w, is white noise. In this thesis, we deal with first order ARX models. 


3. Recursive Estimation 
Real-time application of identification algorithms is interesting for various 
purposes such as supervision, tracking of time-varying parameters for adaptive control, 
filtering, prediction, signal processing, detection, diagnosis and artificial neural networks 
[Ref. 9]. However, most identification methods based on a set of measurements are not 
suitable for real-time application. It is, therefore, desirable to make a suitable 
reformulation of the algorithms in order to provide efficient procedures. 

There are several attractive features of recursive estimation. It is obviously suitable 
for real-time applications, and only a few data points need to be stored. It is thus a 
method which is attractive also as a computational method of off-line algorithms. In 
particular, it provides a method for identification of systems with time-varying 
parameters. 

There are also certain drawbacks such as the fact that the model structure is 
determined a priori and the fact that iterative solutions based on large data sets may be 
difficult to organize. Thus, it is of some interest to consider the desirable modifications 
for real-time application of algorithms originally stated as off-line methods. The 


following study shows one such simple derivation. 
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Consider an ARX model of the form 


A(z ')y(t)=B(z u(t) +e() (3.13) 
with u(t), y(t) input, output sequences, and e(t) zero mean white noise. Also, A and B are 


polynomials in the delay operator z” as 


A(z™)=1+a,z7'+...+4,27! 
(3.14) 
B(z"')=b,z'+...+b,2* 
The problem is to estimate the parameters a; and b; from the data {u} and {y} in a 


recursive fashion. In order to do so, we put the model, in equation 3.13, in Regression 


form as 


Wt)=@ "(08 +e(0) 
2" =[ -yXt —1),.0 -y(t -n),u(t- 1),...54(t -n)] 


B=[0, 4.0.58 50005D,) 


In the light of equation 3.15 we can apply Kalman filtering techniques to estimate 0. If 


we assume, @ is a constant vector, we can write the following state space equations 


O(t+1)=8(¢) 
(3.16) 


yt)= 2 7(HO(1) +e(2) 


Notice that equation 3.26 is a standard state space equation, where e(t) is white noise, 


and we can easily apply Kalman filtering for the estimation of §(t) as 








O(t+1)=8()+KODO- 2760) 


__ POR® 
0+ @ (OPH) oy 


a = Pee (OP(t 
Pt+1)=P- OLOL OPO 
47+ OH)PO) 
with \? =E(e(t)’]. It seems that in equation 3.17 we need to know’; the covariance 
of the error e(t), which is usually not known. However, in practice we do not need )? . 


Just define 
py=P 8 (3.18) 
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The rest is an easy exercise to show that the recursive least squares estimation method 


can be written as 


6(t+1)=6() +K Ly) -276(0)) 


PODRO 
1+o@POSO (3.19) 


P(t+1)=P(t)- POHOL7OP) 
1+O7)POOO 
Therefore, P(t) is the error covariance matrix,and it is initialized as P(O)=pJ, with I 
identity matrix, p, a large positive value. 
4. Simulation Results 


So far, we assumed the system dynamics introduced in the following 


differential equations are known. 











v=-av+Bu, 
6=-y6+ou, 


(3.20) 


At this point, we try to estimate the parameters a, 8, y, and o by means of RLS, 
explained above. During the estimation process, we try to fit our model to a first order 
ARX model by using the sampled values of V, u,, 8, and u,. After off-line estimation of 
discrete parameters (see Fig. 3.10), we transform discrete time parameters to continuous 
time parameters by Laplace Transforms and obtain our a, 8, y, and o parameters, 
which will be used in further simulations. The source code used to do the simulations 
described above are written in MATLAB and listed in Appendix-B. 

The results of the estimation for different initial conditions are shown in Figures 
3.11 through 3.18 , where we used the data collected for seven successive 360° scans. 
In all the figures, the sequence of the estimated reflected surfaces is shown for each of 
the scans superimposed to the contours of the potential function. In Figures 3.11 through 
3.14, we assume the initial position and orientation of the sonar head to be known, but 
the velocity and direction have uncertainties. In Figures 3.15 through 3.18, we repeat the 
same simulations with a significant error in the initial estimate of location. In Figures 
3.13 and 3.17, we introduce one slow moving object on portside of our vehicle, and two 
slow moving objects in Figures 3.14 and 3.18 on both sides of the vehicle. As a result 
of these simulations, we conclude that our model is able to recover by using potential 
function approach even when reasonable numbers of obstacles, which are not on the map, 


are present in the operation area. 
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Figure 3.10 Off-Line Estimated Discrete Time Parameters 
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Figure 3.11 Estimation with known initial location, no obstacles are present. 
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Figure 3.12 Estimation with known initial location, no obstacles are present. 
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Figure 3.13 Estimation with known initial location, one obstacle is present. 
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Figure 3.14 Estimation with known initial location, two obstacles are present. 
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Estimated Trajector 
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Figure 3.15 Estimation with unknown initial location, no obstacles are present. 
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Figure 3.16 Estimation with unknown initial location, no obstacles are present. 
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Figure 3.17 Estimation with unknown initial location, one obstacle is present. 
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Figure 3.18 Estimation with unknown initial location, two obstacles are present. 
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IV. SUMMARY, CONCLUSIONS, AND RECOMMENDATIONS 


A. SUMMARY 
This thesis presents a study of model-based estimator for small autonomous 

underwater vehicles. The approach taken in the design and testing of the estimator 
included: 

@ The development of linearized models 

@ The development of potential function for the environment 

@ The estimation of parameters by means of an ARX model 

@ The programming in MATLAB 


@ Simulation studies using additive white Gaussian noise 


B. CONCLUSIONS 
In this study, an estimator which uses knowledge of the vehicle dynamics is 
developed. In particular, position estimation is obtained by combining the sonar range 
information with inertial measurements. 
The following conclusions can be drawn from the results of this study: 
®@ Position, velocity and pitch rate estimation is possible. 


@ A piecewise constant Extended Kalman gain is adequate for estimator. 


@ Estimation of states by Potential function technique is possible. 








C. RECOMMENDATIONS 
The algorithm explored in this thesis could be used in the interim for the NPS AUV 
II during pool missions. It is therefore recommended that the following be accomplished 
to facilitate implementation of the estimator: 
@ Convert the estimator’s main loop and other MATLAB functions to C 
and compile the entire program for use in the NAPS AUV If computer. 
@ Find a better way of identifying potential functions for non-geometrical 
environments. 
@ Implement neurol networks to identify potential functions. 
® Try to eliminate an extra controller by using potential functions in 


obstacle avoidance problem. 
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APPENDIX A 


A. PROGRAM STRUCTURE 

The programs are PRDATA4.M, RANGEH.M, and AUVS.M. The names of the 
source code files for different versions of the programs are appended with the version 
number, for example, PRDATA4.M. 

AUVS.M is the main source code, which initializes the filter parameters, the 
simulated vehicle state, the Extended Kalman filter state, the control input, and the #, 
A,, and H matrices of the linearized model. The program reads the data file as a standard 
MATLAB matrix instead of running a separate vehicle model to generate simulated 
measurements. 

PRDATA4.M is the source code, which generates the simulated measurements. It 
also saves the measurements into a data file as a standard MATLAB matrix. 

RANGEH.M is the source code, which generates the estimated nonlinear 
measurements of range. It also calculates the gradient of range with respect to heading 


and position of the vehicle. RANGEH.M is called by both PRDATA4.M and AUVS.M. 
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B. PROGRAM LISTING FOR SIMULATIONS IN CHAPTER II 


HKHKHLGHHKKKKHKRKDUKKRNKNNNHKHKEKNNDHDHH HHH % HG % 
File AUVS.M% 
This little program is designed for estimating the position of an AUV which 
moves in horizontal plane. We assume that AUV starts from a known 
position with a constant heading and constant speed. In this model we also 
use RPM and RUDDER angle as the inputs to the system. 


Calls RANGEH.M 


Modified 26 Jun 93 


BR a a a af a8 af af af a af 


Ver.5 
LhDKELNNNNHNHMNGKHRKRNHMHNKNHNHNHGDH HGH GH % % % 


clear 
clg 
cle 


L1 =6;L2=6; %pool dimensions 


Ts=0.0225; %entering sampling time 
load datae25.dat 

clear D 

D=datae25; 

kmax =length(D) + 1; %discrete time 
x0=[.8,1.2,0,1,0)’; %initilization of states 
xkik=zeros(5,kmax); 

xk1k(:,1)=x0; 

PO=diag({10 10 0 10 0)); 

pkik=P0; 

R=0.1; 

alfa=1; 

beta =0.001; 

sigma=1; 

gama=1; 


for k=1:kmax-1 
headh=xk1k(4,k);  %heading of vehicle 
theta=D(k, 1); 
rho=D(k,2); 
RPM(k) = D(k,6); 
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RUDDER({k) =D«k,7); 


a=cos(headh*pi/ 180); 
b=sin(headh *pi/180); 
A=[ 00a00 

00b00 

0 0 -alfa 0 0 

00001 

00 0 O -gama); 
B=[0 0;0 0;beta 0;0 0;0 sigma); 
{phi,del1]=c2d(A,B,Ts); 


shdg =theta+ headh; 


if shdg > 180 
shdg =-360+ shdg; 
end 


{rhoh, ho] =rangeh(xk1k(1 yk), xk1k(2,k),shdg,L1,L2); %sonar range 
drdx =ho(1,2); 

drdy =ho(1 ,3); 

h=[drdx drdy 0 0 0); 

KG=pkik*h’/(h*pkik*h’ +R); 

pkikl =(eye(5)-KG*h)*pkik; 

pk1k=phi*pk1k1 *phi’; 

xk1k1(:,k) =xk1k(:,k) +KG*(tho-rhoh); 
xkik(:,k+1)=phi*xk1k1(:,k)+dell *[RPM(k) RUDDER(k)]’; 


xp(k) =xk1k1(1 ,k) +rhoh*cos(shdg *pi/1 80); 

yp(k) =xk1k1(2,k) +rhoh*sin(shdg*pi/ 180); 
end 
axis([-1 L1+1 -1 L2+1)) 
plot(xk1k1(1,:),xk1k1(2,:),D¢,4),DC 5),’8 XD, yP,’*’), grid 
xlabel(’x’), ylabel(’y’), title? POOL’) 
!del tezg25.met 
meta tezg25 
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EEL %%%% % HM HH % HH % % % % % % HH % HK % % % % % % % KK % 
File PRDATA4.M 


This little program is designed for creating data.We assume that AUV starts 
from a known position with a constant heading and constant speed.We also 
use the RPM and RUDDER angle as the input. In this program we also added 
small noises to the states. 


Calls RANGEH.M 
Modified 26 Jun 93 


%  Ver.4 
% 
%H%HHDHHKRNRKDKRKRRNHKNKHHMHHHKMGHNMHHHHHHEHH HG % 


clear 
clg 
cle 


L1 =6;12=6; %pool dimensions 


Ts=0.0225; entering sampling time 
Tf=9; %time required for a 360 degrees return 
kmax =9*Tf/Ts; %discrete time 
x=zeros(5,kmax); 

x, D=[1 10315 Oy; 

RPM = 150*ones(1 ,kmax); 

RUDDER = zeros(1 ,kmax); 

alfa=1; 

beta=0.001; 

sigma =1; 

gama=1; 


for k=1:kmax-1 
tetha=x(4,k); 
a=cos(tetha*pi/ 180); 
b=sin(tetha*pi/180); 





A=[ 00a00 

00b00 

0 0 -alfa0 0 

00001 

0 0 0 0 -gama]; 
B=[0 0;0 0;beta 0;0 0;0 sigma); 
E=eye(5); 
{phi,del1] =c2d(A,B,Ts); 
[phi,del2] =c2d(A,E,Ts); 


RUDDER(k)=5; 


rand(’normal’) 

ex =0.01 *rand; 

ey =0.01 “rand; 

ev=0.01 *rand; 

et=0.01*rand; 

etd =0.01 *rand; 

x(:,k+1)=phi*x(:,k) +dell*[RPM(k) RUDDER(k)]’ +del2*[ex ey ev et etd)’; 
head(k) =x(4,k); 


shdg(k) = rem(head(k) +0.9*k,360); %sonar heading 
shdg1(k) =rem(0.9*k,360); 
if shdg(k) > 180 
shdg(k) =-360+ shdg(k); 
end 
if shdgi(k) > 180 
shdg1(k) =-360+ shdg1 (k)+0.01 *rand; 
end 
{r,h] =rangeh(x(1,k),x(2,k),shdg(k),L1,L2); %sonar range 
dist(k) =r+0.01*rand; 
temp(k, :)=[shdg1(k) dist(k) x(3,k) x(1,k) x(2,k) RPM(k) RUDDER(k) x(4,k) x(5,k)]; 


end 
!del datae33.dat 
save datae33.dat temp /ascii %saving data file in ASCII code 


axis(’square’) 

axis({0 L1 0 L2)) 
plot(x(1,:),x(2,:)),grid 
title’ POOL’) 
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KRKKKHKKMKDG%%HKKRKEKENMNMKNNNKHKKKRKNRNNN KKK EG 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 


HKKLNKKDKLKNKNhLKDN KKK %% HK % % % K % % % 


File RANGEH.M 


[r,h) =range(x,y,theta,L1,L2) 

computes the sonar range (r) and its gradient (h) 
at position (x,y) with heading theta 

in the L1 by L2 pool. 

h=[dr/dtheta, dr/dx, dr/dy] 


Modified 26 Jun 93 


Called by AUVS.M and PRDATA4.M 


function [r,h] =rangeh(x, y,theta,L1,L2) 
theta =theta*pi/180; 

thi =atan2(-y,-x); 

th2 =atan2(L2-y,-x); 

th3 =atan2(L2-y,L1-x); 

th4 =atan2(-y,L1-x); 


while (theta > th! +2*pi), 
theta= ~-2*pi; 
end 


while (theta < =th4), 
theta=theta+2*pi; 

end 

c=cos(theta);s =sin(theta); 


if (theta > =th2)&(theta < thl +2*pi), 
r=-x/c; 
h=[-x*s/c*2,-1/c,0]; 

end 


if (theta > =th3)&(theta < th2), 
r=(L2-y)/s; 
h=[(y-L2)*c/s*2,0,-1/s}; 
end 


if (theta > =th4)&(theta < th3), 
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r=(L1-x)/c; 
h=[(L1-x)*s/c*2,-1/c,0); 


if (theta > =thl +2*pi)&(theta < th4+2*pi), 
=-y/s; 
h=[{-y*c/s*2,0,-1/s]; 
end 
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A. PROGRAM STRUCTURE 


The programs are SIMULS.M, SCANS.M, VP5.M, SIGMOID.M, DSIG.M, 
SHOWS.M, SIMUL8.M, and SCAN8.M. The names of the source code files for 
different versions of the programs are appended with the version number, for example, 
SIMUL8.M. 

SIMULS.M is the main source code, which initializes the filter parameters, the 
simulated vehicle state, the Extended Kalman filter state, the control input, and the ®, 
A,, and H matrices of the linearized model. The program reads the data file as a standard 
MATLAB matrix instead of running a separate vehicle model to generate simulated 
Measurements. In this source code the dynamic parameters of the vehicle are assumed 
to be known. 

SCANS.M is designed for estimating the states of the vehicle using the potential 
function approach. It scans the estimated borders of the map. VP5.M, SIGMOID.M, 
DSIG.M, and SHOWS.M are designed to calculate the value of the potential function, 
the value of sigmoid function, the derivatives of sigmoid function and to show the 
graphics respectively. 

SIMUL8.M and SCAN8.M are basically same with the SIMULS.M and 
SCANS.M. The only difference introduced in these versions is the estimation of the 


dynamic parameters of the vehicle by using an ARX model and RLS method. 
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B. PROGRAM LISTING FOR SIMULATIONS IN CHAPTER II 


KH MNNHKNNNKG KKK KH % HK % % % KK % KK % % % % % % % % % % % % 

File SIMULS.M 
This source code is designed for estimating the position of an AUV which 
moves in horizontal plane. We assume that AUV starts with a constant 
heading and speed. In this model we use RPM and RUDDER angle as the 
inputs to the AUV system. 

Calls SCANS.M, SHOWS.M 

Modified 9 Jul 93 


Ver.5 
%%%%%%%ERH%SHH HHH HHHRHHHHH HMMM MN 


% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 


clear; 

clg; 

hold off; 

cle 

subplot(221) 

load c:\mat\tez\data\datae32.dat % data of pool 
data =datae32; 

n=length(data); 


nl=1; n2=400; 
lambda =50; 
zhm =zeros(5, 1); 
P=diag({10,10,10,10,10]); zhO=[3,1,0,0,0)’; % initialize estimate 
i=0; 
while n2<n, 
{zh,P,zs] =scan5(zh0,P,data(n1:n2,:), lambda); 
xm=zs(1,:); ym=zs(2,:); 
zhm =[{zhm, zh]; 
i=i+1; 
show5 
if i==4 
meta tezg37 





end 

hold off 

nl =n2; n2=min({n! +400,n)); 

lambda =0.5*lambda; 

nz=length(zh); zhO=zh(-: nz); 
end 


hold off 

show5 

hold on 

plot(zhm(1,:), zhm(2,:),’*g’), title’ Estimated Trajectory’) 
meta 

end 
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BEEK HHKKENKNKKRKEEKEKRKREG KEE KG HHH % HK % % % KH % % 
File SCANS.M 


This source code is designed for estimating the position for static point using 
potential fuction. It scans the estimated borders of the map 


Modified 8 Jul 93 


% 
% 
% 
% 
% 
% Calls VP5.M 
% 
% 
% 
% Ver.5 
%%%%%%HHH%HHHHHHRHRHRHNNHNHNNKN KN %% HG % KH % 
function [zh,P,zs]=scanS(zh0, P, data, lambda) 
n=length(data); 
zh(: ,1)=zh0; 
R=1; 
alfa=11.2957;beta=.0111;sigma=1.0023;gama=1.0023;Ts =0.0225; 
for t=1:n-1 
rho=data(t,2); theta=data(t,1); headh=zh(4,t); 
RPM(t) =data(t,6); RUDDER(t) =data(t,7); 
a=cos(headh*pi/180); b=sin(headh*pi/ 180); 
A=[00a00 
00b00 
0 O -alfa 0 0 
00001 
0 0 0 O -gama]; 
B=[0 0;0 0;beta 0;0 0;0 sigma]; 
[Phi,del] =c2d(A,B,Ts); 
dx0=rho*cos((theta+ headh) *pi/180); 
dy0=rho*sin((theta + headh)*pi/180); 
x0=zh(1 ,t)+dx0; 
yO=zh(2,t) +dy0; 
zs(:,t)=[x0;y0); 
[v,dvx,dvy] = VP5(x0, yO, lambda); 
h=[dvx,dvy,0,(-dvx*dy0+dvy*dx0)*pi/180,0)’; 
s=h’*P*h+1; K=Phi*P*h/s; 
e=0-v; 
zh(: t+ 1) =Phi*zh(: ,t)+del*[RPM(t) RUDDER(t)]’ +K*e; 
P=Phi*P*Phi’-K*s*K’; 
end 
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RHVLBKKKKKLELN$KNNNNND DKK KKH % GH % % % Hh % % % 
File VP5.M, SIGMOID.M, DSIG.M, SHOWS.M 


These source codes are designed for calculating the potential function, the 
value of sigmoid, its derivative at a given point and to show graphics. 


Modified 9 Jul 93 


% 
% 
% 
% 
% 
% 
% 
% Ver.5 
%%%ERGRDG%%%%NH%H%RH%%KHNHNNNHNNHNHDH HNN % HM % 
function [v,dvx,dvy] = VPS5(x,y, lambda); 

vO = (x. *(x-6))*(y. *(y-6))’; 

z=(v0)/(lambda); v=sigmoid(z); 

dvx =dsig(z)*((x-6)*(y. *(y-6))’ +x*(y. *(y-6))’)/lambda; 

dvy =dsig(z)*((x.*(x-6))*(y-6)’ + (x. *(x-6))*y’)/lambda; 

end % VP5 


function y =sigmoid(x) 
x=min(x, 100); x=max(x,-100); 
y= (1-exp(-x))./(1 +exp(-x)); 
end % SIGMOID 


function d=dsig(x) 

% derivative of sigmoid 

x=min(x, 100); 

x =max(x,-100); 

temp=exp(-x); 

d=temp ./ (1+2*temp + temp .* temp); 
end %DSIG 


%SHOWS.M 
=-1:.1:7; 
y=-1:.1:7; 
[v,dvx,dvy] = VP5(x’,y’ lambda); 
contour(v,x,y) 
hold on 
for t=1:length(xm) 
plot(xm(t), ym(t), ’og’) 
end 
end %show 





%%% GHG Rh %% HH b % % % % % % % % HK % % Kh % % % K % To % he % % % 


% 

% File SIMUL8.M 

% 

% This source code is designed for estimating the position of an AUV which 
% moves in horizantal plane. We assume that AUV starts with a constant 
% heading and speed. In this model we use RPM and RUDDER angle as the 
% inputs to the AUV system. Also we try to fit the heading rate and speed data 
% to an ARX model by using RLS. 

% 

% Calis SCAN8.M, SHOWS.M 

% 

% Modified 10 Aug 93 

% 

% Ver.8 
%E%%RG%%HHHEKRNNKDNNKHEKN DMM HM KH % % % % % % % % 
!del tezg46. met 

!del para8.met 

clear; clg; hold off;cic 

subplot(221) 

load datae25 .dat 

data = datae25; 

n=length(data); 

Ts=0.0225; 


%ESTIMATION OF SYSTEM PARAMETERS BY USING RLS AND 
%FIRST ORDER ARX MODEL 


% ESTIMATION FOR SPEED PARAMETERS 
vi =data(: ,3); % MEASURED SPEED DATA 
ul =data(: ,6); %RPM INPUT 


th] =zeros(2,n); 
Pi =10*eye(2); 
for k] =2:n-l 
phitl(k1,:)=[-vi(k1-1) ul(k1-1)); 
den1(k1)=1 + phit1(k1,:)*P1 “phit1(k1,:)’; 
K1(: ,k1)=P1 *phit1(k1,:)’/den1(k1); 
Pl =P1-P1 *phit1(k1,:)’ *phit! (k1,:)*P1/den1(k1); 
thi(:,k1+1)=th1(:,k1)+K1(:,k1)*(vi(k1)-phit1(kKI,:)*thi¢,k1)); 
end 
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%ESTIMATION FOR YAW RATE PARAMETERS 
v2 =data(: ,9); %MEASURED HEADING DATA 
u2 =data(: ,7); % RUDDER ANGLE INPUT 


th2 =zeros(2,n); 
P2=10*eye(2); 
for k2=2:n-1 
phit2(k2,:)=[-v2(k2-1) u2(k2-1)]; 
den2(k2) = 1 + phit2(k2,:)*P2*phit2(k2,:)’; 
K2(: ,k2)=P2*phit2(k2,:)’/den2(k2); 
P2=P2-P2*phit2(k2,:)’*phit2(k2, :)*P2/den2(k2); 
th2(: ,k2+ 1) =th2(: ,k2)+K2(: ,k2)*(v2(k2)-phit2(k2,:)*th2(: ,k2)); 
end 


%TRANSFORMING PARAMETERS FROM DISCRETE TO CONTINUOUS TIME 
phil =[-th1(1,n) 0;0 -th2(1,n)]; 

dell =[th1(2,n) 0;0 th2(2,n)]; 

[A1,B1]=d2c(phil ,del1 , Ts); 

prms=[-A1(1,1);B1(1,1);-A1(2,2);B1(2,2)); 


%PLOTING THE DISCRETE PARAMETERS 
tl =0:n-1; 
t=0.0225*t1; 


plot(t,-th1(1,:));title? ALFA ESTIMATE’);xlabel(’Time’); grid 
plot(t,th1(2,:));title’ BETA ESTIMATE ’);xlabel(’Time’); grid 
plot(t,-th2(1,:));title’ GAMA ESTIMATE ’);xlabel(’Time’);gric 
plot(t,th2(2,:));title’ SIGMA ESTIMATE ’); xlabel(’Time’); grid 
pause 

meta para9 

clg 


#SIMULATION DUE TO PARAMETERS ESTIMATED ABOVE 


axis(’normal’) 
subplot(221) 
nl=1; n2=400; 
lambda =200; 
zhm =zeros(5, 1); 


P=diag([100, 100,100, 100, 100]); zhO=(2,3,1,10,10]’; % initialize estimate 


i=0; 
while n2<n, 
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(zh, P,zs}] =scan8(zhO,P,data(n1 :n2,:),lambda,prms); 

xm =zs(1,:); ym=zs(2,:); 

zhm =[zhm, zh]; 

ix=i+l; 

show5 

ifi== 

meta tezg46 

end 

hold off 

nl=n2; n2=min((ni +400,n)); 

lambda =0.5*lambda; 

nz=length(zh); zhO=zh(: ,nz); 
end 


hold off 

show5 

hold on 

plot(zhm(1,:), zhm(2,:),’*g’), title? Estimated Trajectory’) 
meta 

end % simulation 

'!del den.dat 

save den.dat zhm /ascii 
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bh % % % % % % % No % % % % % % % % MK % % % % % % % % To % Ko KH KK K % % % % % 
File SCAN8.M 


This source code is designed for estimating the position for static point using 
potential fuction. It scans the estimated borders of the map 


Calls VP5.M 
Modified 8 Aug 93 


Ver.8 
%%KRH%NHKNNKE%%HMNHHNNKHNNNEGNG SM HG % % % % % 


BW B® VF at at aF at af a af af az 


function (zh, P,zs] =scan8(zh0,P,data,lambda,prms) 
n=length(data); 
zh(:, 1) =zh0; 
R=1; 
alfa=prms(1);beta=prms(2);sigma=prms(3); gama =prms(4);Ts =0.0225; 
for t=1:n-1 
rho=data(t,2); theta=data(t,1); headh=zh(4,t); 
RPM(t) =data(t,6); RUDDER(t) =data(t,7); 
a=cos(headh*pi/180); b=sin(headh*pi/180); 
A=[00a00 

00b00 

0 0 -alfa 0 0 

00001 

0 0 0 O -gamaj; 
B=[0 0;0 0;beta 0;0 0;0 sigma]; 
[Phi,del] =c2d(A,B,Ts); 
dx0=rho*cos((theta + headh) *pi/ 180); 
dy0=rho*sin((theta+ headh)*pi/ 180); 
x0=zh(1 ,t) +dx0; 
yO=zh(2,t) +dy0; 
zs(:,t)=[x0;y0]; 
{v,dvx,dvy]=VP5(x0,y0,lambc .); 
h=[dvx,dvy,0,(-dvx*dy0+dvy*dx0)*pi/180,0)’; 
s=h’*P*h+1; K=Phi*P*h/s; 
e=0-v; 
zh(: t+ 1)=Phi*zh(- ,t)+del*{RPM(t) RUDDER(t)]’+K*e; 
P=Phi*P*Phi’-K*s*K’; 
end 
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